'' Octopilot -- ServoIO.spin
'' (c) 2008-2009 Ben Levitt
'' http://code.google.com/p/octopilot/
''
'' This is the sensory and motor cortex of the Octopilot brain.
'' It reads in values from the receiver channels (up to 8) and sets the dataInReady flag.
'' The other objects work together to process this data and determine what the output
'' servo values should be, and then set the dataOutReady flag.  This code then sends out
'' the calculated values to the servos, and starts again.


OBJ
  eeprom : "Propeller Eeprom"


CON 
  ' Constants are defined in ASM at the bottom of this file


VAR
  word inVals[8]
  word outVals[8]
  word defaultVals[8]
  word failsafeThreshold
  byte failsafeChannel
  byte numInChannels
  byte numOutChannels
  byte inBasePin
  byte outBasePin
  byte ledPin
  byte dataInReadyFlag  ' 1= Ins  are ready
  byte dataOutReadyFlag ' 1= Outs are ready
  byte hasSetDefaults


PUB Start(inChannels, outChannels, inBase, outBase, led, failsafeChannelNum, failsafeThresholdVal)
' Takes number receiver channels (in), and number of servo channels (out)
' Takes the base pin for ins ad for outs, and the servo led pin nimber
' Takes a failsafe channel and threshold used to determine if the receiver has lost contact with the transmitter
  numInChannels := inChannels
  numOutChannels := outChannels
  inBasePin := inBase
  outBasePin := outBase
  ledPin := led
  failsafeChannel := failsafeChannelNum
  failsafeThreshold := failsafeThresholdVal
  dataInReadyFlag := 0
  dataOutReadyFlag := 0
  hasSetDefaults := 0
  
  ' Previously saved EEPROM values are used as the initial values at startup, so if defaults are 0,
  ' then it hasn't been set yet, so set defaults to 15000, except failsafe value, which we set to 10000.
  if defaultVals[0] == 0
    ' Start Ins, Outs, and Defaults at sane values.
    wordfill(@defaultVals, 15000, 8)
    wordfill(@inVals, 15000, 8)
    wordfill(@outVals, 15000, 8)
    defaultVals[failsafeChannel] := 10000
  else
    ' Old defaults are already available. Use them for initial Ins and Outs.
    wordmove(@inVals, @defaultVals, 8)
    wordmove(@outVals, @defaultVals, 8)
    
  ' Start Ins[failsafe] at 0, since we watch that to check for real defaults and receiver status.
  inVals[failsafeChannel] := 0
   
  
  ' If there are extra Outs, set their matching In values to center.
  ' This is useful for the emergency assembly passthrough mode since these will not otherwise get updated from 0.
  if outChannels > inChannels
    wordfill(@inVals + 2*inChannels, 15000, outChannels - inChannels)
  
  cognew(@Setup, @inVals)


PUB GetAddresses(inValsAddr, outValsAddr, defaultValsAddr, dataInReadyAddr, dataOutReadyAddr)
' Pass back addresses of data by reference
  LONG[inValsAddr] := @inVals
  LONG[outValsAddr] := @outVals
  LONG[defaultValsAddr] := @defaultVals
  LONG[dataInReadyAddr] := @dataInReadyFlag
  LONG[dataOutReadyAddr] := @dataOutReadyFlag


PUB CheckForNewDefaultValues
  ' Get initial servo values to use as defaults
  ' Wait to get them until the failsafe channel is above the failsafe threshold (which means that we're receiving
  ' signal from the transmitter, when the receiver's failsafe is set below the baseline)
  if hasSetDefaults == 0 and IsFailsafeBelowThreshold == 0                      ' Wait for real transmitter data
    repeat until dataInReadyFlag <> 0                                           ' Sync to next servo cycle
    dataInReadyFlag := 0
    wordmove(@defaultVals, @inVals, numInChannels)
    dataOutReadyFlag := 1
    
    ' Set up default values for outputs (so unset servos will use a reasonable default)
    wordmove(@outVals, @defaultVals, numOutChannels)
    
    ' Store defaults to EEPROM
    eeprom.VarBackup(@defaultVals, @defaultVals+15)
    
    hasSetDefaults := 1


PUB GetSwitchState(servoNum, numStates) | duration
' servoNum : which servo
' numStates: 2 for a normal switch, 3 for a tristate switch
  duration := inVals[servoNum]
  if numStates == 2
    if duration => 15000
      return 1
  elseif numStates == 3
    if duration > 17000
      return 2
    elseif duration > 12000
      return 1
  return 0


PUB IsFailsafeBelowThreshold
  if inVals[failsafeChannel] < failsafeThreshold
    return 1
  return 0


DAT
                        org
'---------------------------------------------------------------------------------------------------------------------
' Copy parameters from main memory to cog memory
Setup                   mov     Index,                  par                     ' Set Index Pointer
                        mov     _inValsAddr,            Index                   ' Get value
                        add     Index,                  #16                     ' Increment Index to next Pointer
                        mov     _outValsAddr,           Index                   ' Get value
                        add     Index,                  #16                     ' Increment Index to next Pointer
                        mov     _defaultValsAddr,       Index                   ' Get value
                        add     Index,                  #16                     ' Increment Index to next Pointer
                        rdword  _failsafeThreshold,     Index                   ' Get value
                        add     Index,                  #2                      ' Increment Index to next Pointer
                        rdbyte  _failsafeChannel,       Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        rdbyte  _numInChannels,         Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        rdbyte  _numOutChannels,        Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        rdbyte  _inBasePin,             Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        rdbyte  _outBasePin,            Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        rdbyte  _ledPin,                Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        mov     _dataInReadyAddr,       Index                   ' Get value
                        add     Index,                  #1                      ' Increment Index to next Pointer
                        mov     _dataOutReadyAddr,      Index                   ' Get value

                        mov     StaleInsCount,          StaleInsLimit           ' Consider data to be stale until we get first nav data
                        mov     StaleOutsCount,         StaleOutsLimit          ' Consider data to be stale until we get first nav data
                        ' Setup ServoPinMask
                        mov     ServoPinMask,           #0                      ' Clear ServoPinMask
                        cmp     ServoPinMask,           #1                wc    ' Set C flag to 1
                        rcl     ServoPinMask,           _numOutChannels         ' Set _numChannels consecutive pins to 1 (C set to 1 in previous line)
                        shl     ServoPinMask,           _outBasePin             ' Shift high pins over to their start at _outBasePin

                        ' Set PinOut Directions
                        mov     temp,                   _ledPin                 ' _ledPin is just a pin number
                        mov     _ledPin,                #1                      ' but it needs to be a single bit set in a pinMask
                        shl     _ledPin,                temp
                        muxc    dira,                   ServoPinMask            ' Set I/O directions to 1 (C is still 1) for out pins
                        muxc    dira,                   _ledPin                 ' DEBUG
                        muxc    outa,                   _ledPin                 ' set LedPin on
                        
                        ' Setup ReceiverPinMask
                        mov     ReceiverPinMask,        #0                      ' Clear ReceiverPinMask
                        cmp     ReceiverPinMask,        #1                wc    ' Set C flag to 1
                        rcl     ReceiverPinMask,        _numInChannels          ' Set _numChannels consecutive pins to 1 (C set to 1 in previous line)
                        shl     ReceiverPinMask,        _inBasePin              ' Shift high pins over to their start at _outBasePin
                        
                        ' Setup ServoPin[1-8]
                        mov     ServoPin1,              #1                      ' Set first pin to 1
                        shl     ServoPin1,              _outBasePin             ' Shift high pin into place at _outBasePin
                        mov     ServoPin2,              ServoPin1               ' Copy to the next ServoPin
                        shl     ServoPin2,              #1                      ' Shift left by one
                        mov     ServoPin3,              ServoPin2               ' Copy to the next ServoPin
                        shl     ServoPin3,              #1                      ' Shift left by one                                   
                        mov     ServoPin4,              ServoPin3               ' Copy to the next ServoPin
                        shl     ServoPin4,              #1                      ' Shift left by one                                   
                        mov     ServoPin5,              ServoPin4               ' Copy to the next ServoPin
                        shl     ServoPin5,              #1                      ' Shift left by one                                   
                        mov     ServoPin6,              ServoPin5               ' Copy to the next ServoPin
                        shl     ServoPin6,              #1                      ' Shift left by one                                   
                        mov     ServoPin7,              ServoPin6               ' Copy to the next ServoPin
                        shl     ServoPin7,              #1                      ' Shift left by one                                   
                        mov     ServoPin8,              ServoPin7               ' Copy to the next ServoPin
                        shl     ServoPin8,              #1                      ' Shift left by one                                   

                        ' Setup ReceiverPin[1-8]
                        mov     ReceiverPin1,           #1                      ' Set first pin to 1
                        shl     ReceiverPin1,           _inBasePin              ' Shift high pin into place at _outBasePin
                        mov     ReceiverPin2,           ReceiverPin1            ' Copy to the next ReceiverPin
                        shl     ReceiverPin2,           #1                      ' Shift left by one
                        mov     ReceiverPin3,           ReceiverPin2            ' Copy to the next ReceiverPin
                        shl     ReceiverPin3,           #1                      ' Shift left by one                                   
                        mov     ReceiverPin4,           ReceiverPin3            ' Copy to the next ReceiverPin
                        shl     ReceiverPin4,           #1                      ' Shift left by one                                   
                        mov     ReceiverPin5,           ReceiverPin4            ' Copy to the next ReceiverPin
                        shl     ReceiverPin5,           #1                      ' Shift left by one                                   
                        mov     ReceiverPin6,           ReceiverPin5            ' Copy to the next ReceiverPin
                        shl     ReceiverPin6,           #1                      ' Shift left by one                                   
                        mov     ReceiverPin7,           ReceiverPin6            ' Copy to the next ReceiverPin
                        shl     ReceiverPin7,           #1                      ' Shift left by one                                   
                        mov     ReceiverPin8,           ReceiverPin7            ' Copy to the next ReceiverPin
                        shl     ReceiverPin8,           #1                      ' Shift left by one                                   
                        
                        ' Self-Modifying code to adjust the channel tested as a failafe.  See LoadFailsafeVal line further below.
                        mov     temp,                   _failsafeChannel        ' start with the failsafe channel number
                        shl     temp,                   #9                      ' shift over 9 bits to put it in the d_field
                        add     CheckFSVal,             temp                    ' add this amount to these three lines below, to adjust the channels that they operate on
                        add     SetFSVal,               temp

                        mov     NextPulse,              #0
                        
                        mov     temp,                   cnt
                        add     temp,                   StartupDelay
                        waitcnt temp,                   StartupDelay            ' Wait a quarter second at startup

'---------------------------------------------------------------------------------------------------------------------
' Read in all servo values simultaneously
'
ReadServoTop            mov     temp,                   NoGlitchIn              'Test to make sure 'cnt' value won't rollover within ServoIn time
                        cmp     temp,                   cnt              wc     'If cnt > NoGlitchIn, write 1 to c flag
              if_c      jmp     #ReadServoTop                                   'If c flag is set get a new Sync Point, otherwise we are ok.
' set (PinsDone) to 0
                        mov     PinsDone,               #0
                        mov     tempTime,               cnt
                        add     tempTime,               MaxServoReadTime
' wait for in pins to all be 0
ReadZeroWait            mov     temp,                   ina
                        and     temp,                   ReceiverPinMask  nr,wz
              if_z      jmp     #ReadServos
                        sub     tempTime,               cnt              nr,wc
              if_c      jmp     #ServoReadTimeout
                        jmp     #ReadZeroWait

' store start time (ReadStartTime)
ReadServos              mov     ReadStartTime,          cnt
' set (LastIns) = 0
                        mov     LastIns,                #0
' set InVal[1-8] to 0
                        mov     InVal1,                 #0
                        mov     InVal2,                 #0
                        mov     InVal3,                 #0
                        mov     InVal4,                 #0
                        mov     InVal5,                 #0
                        mov     InVal6,                 #0
                        mov     InVal7,                 #0
                        mov     InVal8,                 #0
' repeat
' wait until ServoPinMask-masked pins are not equal to (LastIns), or until cnt - ReadStartTime > MaxServoReadTime                   
ReadWaitChanged         mov     tempTime,               ReadStartTime
                        add     tempTime,               MaxServoReadTime
ReadWaitChangedLoop     mov     temp,                   ina
                        and     temp,                   ReceiverPinMask
                        xor     LastIns,                temp             nr,wz
              if_nz     jmp     #WaitChangedDone
                        sub     tempTime,               cnt              nr,wc
              if_c      jmp     #ServoReadTimeout
                        jmp     #ReadWaitChangedLoop
'  save time to (PinChangedTime)
WaitChangedDone         mov     PinChangedTime,         cnt
'  save the xor of old&new pin states to PinDiff
                        mov     PinDiff,                LastIns
                        xor     PinDiff,                temp
'  save the new masked pin value to (LastIns)
                        mov     LastIns,                temp

'  for each in pin (unroll loop)
'   if the pin turned on (if !(PinDiff & ReceiverPinN) skip to the next pin, if (LastIns & ReceiverPinN) skip to TurnedOff)
'    subtract (PinChangedTime) from (InValN) (so InValN = -PinChangedTime)
'   if the pin turned off
'    add (PinChangedTime) to (InValN) (so InValN = pinOffTime - pinOnTime)
'    set this pin's bit in (PinsDone)
CheckPin1               and     PinDiff,                ReceiverPin1     nr,wz
              if_z      jmp     #CheckPin2
                        and     LastIns,                ReceiverPin1     nr,wz
              if_nz     sub     InVal1,                 PinChangedTime
              if_z      add     InVal1,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin1
              
CheckPin2               and     PinDiff,                ReceiverPin2     nr,wz
              if_z      jmp     #CheckPin3
                        and     LastIns,                ReceiverPin2     nr,wz
              if_nz     sub     InVal2,                 PinChangedTime
              if_z      add     InVal2,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin2
              
CheckPin3               and     PinDiff,                ReceiverPin3     nr,wz
              if_z      jmp     #CheckPin4
                        and     LastIns,                ReceiverPin3     nr,wz
              if_nz     sub     InVal3,                 PinChangedTime
              if_z      add     InVal3,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin3
              
CheckPin4               and     PinDiff,                ReceiverPin4     nr,wz
              if_z      jmp     #CheckPin5
                        and     LastIns,                ReceiverPin4     nr,wz
              if_nz     sub     InVal4,                 PinChangedTime
              if_z      add     InVal4,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin4
              
CheckPin5               and     PinDiff,                ReceiverPin5     nr,wz
              if_z      jmp     #CheckPin6
                        and     LastIns,                ReceiverPin5     nr,wz
              if_nz     sub     InVal5,                 PinChangedTime
              if_z      add     InVal5,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin5
              
CheckPin6               and     PinDiff,                ReceiverPin6     nr,wz
              if_z      jmp     #CheckPin7
                        and     LastIns,                ReceiverPin6     nr,wz
              if_nz     sub     InVal6,                 PinChangedTime
              if_z      add     InVal6,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin6
              
CheckPin7               and     PinDiff,                ReceiverPin7     nr,wz
              if_z      jmp     #CheckPin8
                        and     LastIns,                ReceiverPin7     nr,wz
              if_nz     sub     InVal7,                 PinChangedTime
              if_z      add     InVal7,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin7
              
CheckPin8               and     PinDiff,                ReceiverPin8     nr,wz
              if_z      jmp     #CheckPinsDone
                        and     LastIns,                ReceiverPin8     nr,wz
              if_nz     sub     InVal8,                 PinChangedTime
              if_z      add     InVal8,                 PinChangedTime
              if_z      muxz    PinsDone,               ReceiverPin8
'  if PinsDone == ServoPinMask, break
CheckPinsDone           cmp     PinsDone,               ReceiverPinMask  wz
              if_nz     jmp     #ReadWaitChanged
                        mov     StaleInsCount,          #0
                        jmp     #ConvertIns

'---------------------------------------------------------------------------------------------------------------------
' If we stopped getting data from the receiver, set InVals to all 0
ServoReadTimeout        add     StaleInsCount,          #1
                        cmp     StaleInsCount,          StaleInsLimit    wc
              if_c      jmp     #ConvertIns
                        mov     temp,                   #0
                        mov     Index,                  _inValsAddr
                        wrlong  temp,                   Index
                        add     Index,                  #4
                        wrlong  temp,                   Index
                        add     Index,                  #4
                        wrlong  temp,                   Index
                        add     Index,                  #4
                        wrlong  temp,                   Index
                        cmp     Index,                  Index            wz      ' set z=1
                        muxz    outa,                   _ledPin                  ' set LedPin on
                        jmp     #SetReadyForNav

'---------------------------------------------------------------------------------------------------------------------
' Convert InVal values from clock cycles to microseconds (just divide each one by 8)
ConvertIns              shr     InVal1,                 #3                      ' shifting right by 3 divides by 8.
                        shr     InVal2,                 #3                      ' this is why we're using values in
                        shr     InVal3,                 #3                      ' tenths of microseconds instead of
                        shr     InVal4,                 #3                      ' microseconds.  Dividing by 8 is 
                        shr     InVal5,                 #3                      ' so much easier than by 80 in asm.
                        shr     InVal6,                 #3
                        shr     InVal7,                 #3
                        shr     InVal8,                 #3
                        
'---------------------------------------------------------------------------------------------------------------------
' Copy ins from cog memory into main memory ins
CopyValsOut             mov     Index,                  _inValsAddr             ' copy InVals to _inValAddrs
                        and     ServoPinMask,           ServoPin1        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal1,                 Index                   ' write to each inValsAddr
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin2        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal2,                 Index                   ' only write the value to memory if that servo is in use
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin3        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal3,                 Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin4        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal4,                 Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin5        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal5,                 Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin6        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal6,                 Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin7        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal7,                 Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin8        nr,wz  ' check if this servo is in use
              if_nz     wrword  InVal8,                 Index

SetReadyForNav          mov     ReadyVal,               #0
                        wrbyte  ReadyVal,               _dataOutReadyAddr       ' set _dataOutReadyAddr to 0   
                        mov     ReadyVal,               #1
                        wrbyte  ReadyVal,               _dataInReadyAddr        ' set _dataInReadyAddr to 1    

                        call    #SendServos                                     ' While we wait for nav data, send a set of servo pulses
                        
'---------------------------------------------------------------------------------------------------------------------
' Check if OutVals are ready, and if not, use old OutVals or new InVals, depending on how long we've been without externally updated Data
WaitForNavStart         mov     temp,                   NoGlitchNav             'Test to make sure 'cnt' value won't rollover within max NavWait time
                        cmp     temp,                   cnt              wc     'If cnt > NoGlitchNav, write 1 to c flag
              if_c      jmp     #WaitForNavStart                                'If c flag is set get a new Sync Point, otherwise we are ok.
              
                        mov     tempTime,               cnt
                        mov     Time,                   tempTime                ' 
                        add     tempTime,               MaxNavDelay             ' the time to stop checking for outData, and use old data
                        add     Time,                   NavDelayChunk           
WaitForNavLoop          waitcnt Time,                   NavDelayChunk           ' wait one more chunk of time (0.25ms)
                        rdbyte  ReadyVal,               _dataOutReadyAddr       ' get dataOutReady (if OutVals are ready, this is 1)
                        cmp     ReadyVal,               #1               wz     ' set Z Flag if OutVals are ready
                        mov     ReadyVal,               #0                      '
              if_z      mov     StaleOutsCount,         #0                      ' if we're ready set StaleOutsCount to 0
              if_z      muxnz   outa,                   _ledPin                 ' set LedPin off
              if_z      wrbyte  ReadyVal,               _dataOutReadyAddr       ' set dataOutReady to 0                          
              if_z      jmp     #CopyValsIn                                     ' and jump to CopyValsIn
                        cmp     tempTime,               cnt              wc
              if_nc     jmp     #WaitForNavLoop                                 ' if we're not yet past the time to stop waiting for data, loop
                        wrbyte  ReadyVal,               _dataOutReadyAddr       ' set dataOutReady to 0                          
                        muxc    outa,                   _ledPin                 ' set LedPin on
                        cmp     StaleOutsCount,         StaleOutsLimit   wc     ' if we're not ready, check if stale count is past its limit
              if_nc     jmp     #CopyInsToOuts                                  ' if we are too stale, copy InVals to OutVals
                        add     StaleOutsCount,         #1                      ' if not, increment StaleOutsCount
                        call    #SendServos                                     ' and reuse old OutVals
                        jmp     #ReadServoTop
CopyInsToOuts           mov     OutVal1,                InVal1                  
                        mov     OutVal2,                InVal2                  
                        mov     OutVal3,                InVal3                  
                        mov     OutVal4,                InVal4                  
                        mov     OutVal5,                InVal5                  
                        mov     OutVal6,                InVal6                  
                        mov     OutVal7,                InVal7                  
                        mov     OutVal8,                InVal8
                        
        ' If the failsafe channel's value drops below the failsafeThreshold, use the default val
        ' OutVal1 in these lines gets updated in the init code to point to the actual failsafeChannel
CheckFSVal              cmp     OutVal1,                _failsafeThreshold  wc
              if_c      mov     Index,                  _defaultValsAddr
              if_c      add     Index,                  _failsafeChannel
              if_c      add     Index,                  _failsafeChannel
SetFSVal      if_c      rdword  OutVal1,                Index

                        jmp     #ConvertOuts                                    ' and convert them back from 10ths of uS to clock cycles
                        
'---------------------------------------------------------------------------------------------------------------------
' Copy from _outValAddr values to OutVals
CopyValsIn              mov     Index,                  _outValsAddr
                        and     ServoPinMask,           ServoPin1        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal1,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin2        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal2,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin3        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal3,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin4        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal4,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin5        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal5,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin6        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal6,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin7        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal7,                Index
                        add     Index,                  #2
                        and     ServoPinMask,           ServoPin8        nr,wz  ' check if this servo is in use
              if_nz     rdword  OutVal8,                Index

'---------------------------------------------------------------------------------------------------------------------
' Convert OutVal values from microseconds to clock cycles (just multiply each one by 8)
ConvertOuts             shl     OutVal1,                #3                      ' Again, multiplying by 8 is way easier
                        shl     OutVal2,                #3                      ' than by 80 in asm.
                        shl     OutVal3,                #3
                        shl     OutVal4,                #3
                        shl     OutVal5,                #3
                        shl     OutVal6,                #3
                        shl     OutVal7,                #3
                        shl     OutVal8,                #3

                        
SendIfNavWasSlow        mov     temp,                   cnt                     'Make sure we're not sending pulses more often than once
                        add     temp,                   MaxPulseFreq            'per PulseFreq clock ticks
                        sub     temp,                   ExtraPulseThreshold
                        cmp     temp,                   NextPulse     wc
              if_nc     call    #SendServos
                        jmp     #ReadServoTop
                        
'---------------------------------------------------------------------------------------------------------------------
' Send vales out to the servos
SendServos              mov     temp,                   cnt                     'Make sure we're not sending pulses more often than once
                        cmp     temp,                   NextPulse     wc        'per PulseFreq clock ticks
              if_nc     jmp     #WaitForCarry
                        mov     temp,                   NextPulse               'If NextPulse was set very high, and cnt has already looped
                        sub     temp,                   cnt                     'back to 0 (so NextPulse - cnt is huge), then don't get stuck.
                        cmp     temp,                   MaxPulseFreq  wc     
              if_c      jmp     #SendServos

WaitForCarry            mov     SyncPoint,              cnt                     'Create a Sync Point with the system counter
                        mov     temp,                   NoGlitchOut             'Test to make sure 'cnt' value won't rollover within Servo's pulse width
                        cmp     temp,                   cnt           wc        'If cnt > NoGlitchOut, write 1 to C flag
              if_c      jmp     #WaitForCarry                                   'If C flag is set get a new Sync Point, otherwise we are ok.

                        mov     NextPulse,              SyncPoint               'Save the time of the pulse
                        add     NextPulse,              MaxPulseFreq            'Add 20ms, to get earliest time of next pulse
                        
                        cmp     temp,                   temp          wz        'Set Z to 1 (Z stays 1 through the end of TightLoop)
                        muxz    outa,                   ServoPinMask            'Set all physical pins in ServoPinMask on.
                                                                                'These will be turned off as they time out.
                        
                        add     OutVal1,                SyncPoint               'Add clk to OutVals to compare against clk as we loop
                        add     OutVal2,                SyncPoint               
                        add     OutVal3,                SyncPoint               
                        add     OutVal4,                SyncPoint               
                        add     OutVal5,                SyncPoint               
                        add     OutVal6,                SyncPoint               
                        add     OutVal7,                SyncPoint               
                        add     OutVal8,                SyncPoint               

                        mov     ServoPins,              #0                      'Set ServoPins to be off for all servo pins
                        
                        mov     temp,                   SyncPoint               'Set temp to end time (SyncPoint plus _minPulse)
                        add     temp,                   MinPulse                'No need to loop until we're at the min pulse width
                        waitcnt temp,                   MinPulse                'so Wait until minPulse is reached
                        
                        mov     temp,                   SyncPoint               'Set temp to end time (SyncPoint plus _maxPulse)
                        add     temp,                   MaxPulse
                        
'----------------------------------------------Start Tight Servo code-------------------------------------------------
TightLoop               cmpsub  OutVal1,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin1               '(4 - clocks) Set ServoPins.Bit0 to "1" if it's time to turn the pin off
                        cmpsub  OutVal2,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin2               '(4 - clocks) Set ServoPins.Bit1 to "1" if it's time to turn the pin off
                        cmpsub  OutVal3,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin3               '(4 - clocks) Set ServoPins.Bit2 to "1" if it's time to turn the pin off
                        cmpsub  OutVal4,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin4               '(4 - clocks) Set ServoPins.Bit3 to "1" if it's time to turn the pin off
                        cmpsub  OutVal5,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin5               '(4 - clocks) Set ServoPins.Bit4 to "1" if it's time to turn the pin off
                        cmpsub  OutVal6,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin6               '(4 - clocks) Set ServoPins.Bit5 to "1" if it's time to turn the pin off
                        cmpsub  OutVal7,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin7               '(4 - clocks) Set ServoPins.Bit6 to "1" if it's time to turn the pin off
                        cmpsub  OutVal8,                cnt           nr,wc     '(4 - clocks) compare system counter to ServoWidth ; write result in C flag
                        muxnc   ServoPins,              ServoPin8               '(4 - clocks) Set ServoPins.Bit7 to "1" if it's time to turn the pin off
                        nop                                                     '(4 - clocks) We actually had one instruction to spare                        
                        muxnz   outa,                   ServoPins               '(4 - clocks) For any bit in ServoPins that is 1, set that physical pin to 0
                        cmp     temp,                   cnt           nr,wc     '(4 - clocks) Determine if cnt has exceeded width of _maxPulse ; write result in C flag
              if_nc     jmp     #TightLoop                                      '(4 - clocks) if the "C Flag" is not set, stay in the current loop
                                                                       ' Total = 80 - clocks  @ 80MHz that's 1uS resolution

                        sub     OutVal1,                SyncPoint               'Subtract SyncPoint back out of OutVals to leave them usable
                        sub     OutVal2,                SyncPoint               'for next time if we need to fall back to old data.
                        sub     OutVal3,                SyncPoint               
                        sub     OutVal4,                SyncPoint               
                        sub     OutVal5,                SyncPoint               
                        sub     OutVal6,                SyncPoint               
                        sub     OutVal7,                SyncPoint               
                        sub     OutVal8,                SyncPoint
                        
SendServos_ret          ret


'---------------------------------------------------------------------------------------------------------------------
' Constants and Variables

' Constants
StartupDelay            long   250_000 * 80             ' 250.00ms in clock time
MaxNavDelay             long    25_000 * 80             '  25.00ms
NavDelayChunk           long       250 * 80             '   0.25ms
StaleInsLimit           long        20                  ' Data is old after 20 misses in a row (20 * ~45ms = ~900ms)      
StaleOutsLimit          long        12                  ' Data is old after 12 misses in a row (12 * ~60ms = ~720ms)        
MaxServoReadTime        long    25_000 * 80             ' 25ms
MaxPulseFreq            long    20_000 * 80             ' 20ms
ExtraPulseThreshold     long    10_000 * 80             ' 10ms

MinPulse                long       850 * 80             '1mS  - 0.15ms
MaxPulse                long     2_150 * 80             '2mS  + 0.15ms 
NoGlitchIn              long  $FFFF_FFFF - 25_000 * 80  'max count - 25.0ms
NoGlitchOut             long  $FFFF_FFFF -  2_500 * 80  'max count -  2.5ms
NoGlitchNav             long  $FFFF_FFFF - 30_000 * 80  'max count - 30.0ms


' Cog-local values of spin variables (remain constant for the life of this cog)
_inBasePin              res     1
_outBasePin             res     1
_ledPin                 res     1
_failsafeThreshold      res     1
_failsafeChannel        res     1
_numInChannels          res     1
_numOutChannels         res     1
_inValsAddr             res     1
_outValsAddr            res     1
_defaultValsAddr        res     1
_dataInReadyAddr        res     1
_dataOutReadyAddr       res     1


' Vars used in asm code
ReadyVal                res     1
StaleInsCount           res     1
StaleOutsCount          res     1
Time                    res     1                       ' used for waitcnts
Index                   res     1
SyncPoint               res     1
ServoPinMask            res     1
ReceiverPinMask         res     1
temp                    res     1

tempTime                res     1
PinsDone                res     1
ReadStartTime           res     1
LastIns                 res     1
PinChangedTime          res     1
PinDiff                 res     1
NextPulse               res     1


' Cog-local values for servo data
InVal1                  res     1
InVal2                  res     1
InVal3                  res     1
InVal4                  res     1
InVal5                  res     1
InVal6                  res     1
InVal7                  res     1
InVal8                  res     1

OutVal1                 res     1
OutVal2                 res     1
OutVal3                 res     1
OutVal4                 res     1
OutVal5                 res     1
OutVal6                 res     1
OutVal7                 res     1
OutVal8                 res     1

ServoPins               res     1
ServoPin1               res     1
ServoPin2               res     1
ServoPin3               res     1
ServoPin4               res     1
ServoPin5               res     1
ServoPin6               res     1
ServoPin7               res     1
ServoPin8               res     1

ReceiverPin1            res     1
ReceiverPin2            res     1
ReceiverPin3            res     1
ReceiverPin4            res     1
ReceiverPin5            res     1
ReceiverPin6            res     1
ReceiverPin7            res     1
ReceiverPin8            res     1
'---------------------------------------------------------------------------------------------------------------------
                        fit                             ' Make sure this all fits into one cog
 